/* mp_core.c -- centro de operações
 *
 * Copyright (C) 2008, Diogo F. S. Ramos <diogofsr@grad.icmc.usp.br>
 *                     Bruno Ribeiro de Oliveira
 *                     Arineiza Cristina Pinheiro
 *                     Tomas Grover
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */
#include <cv.h>
#include <highgui.h>
#include <math.h>

#include "mp_core.h"
#include "mp_utils.h"

void
mp_init( const char *server,
	 const char *port )
{
    marco_polo_t * explorer = NULL;
    int iport = 0;
    int k = 0;
    double obstacle = 0;

    sscanf( port, "%d", &iport );
    explorer = mp_new( server,
		       iport,
		       "world_map");

    if( explorer == NULL )
	xerror_fatal("Não foi possível iniciar o grande Explorador");

    while(1)
    {
	int i = 0;
	int x = 0;
	int y = 0;

	double obstacle_global_angle = 0;
	double laser_angle = 0;
	

	playerc_client_read( explorer->client );

	/* HACK para atrasar a atualização posição */
	if( k == 0 )
	    k++;
	else
	{
	    mp_update_position( explorer );
	    k = 0;
	}

	/* Desenha o mapa da região */
	for( i = 0 ; i <= 360 ; i++ )
	{
	    obstacle = explorer->laser->scan[i][0];
	    laser_angle = explorer->laser->scan[i][1];
	    obstacle_global_angle =
		mp_laser_to_global_rad( (double) explorer->position2d->pa,
					(double) laser_angle );
	    mp_map_x_y_obstacle( explorer,
				 obstacle, 
				 obstacle_global_angle,
				 &x,
				 &y);
	    
	    /* Marca o espaço vazio */
	    mp_map_mark_open_space( explorer,
				    x,
				    y );
	    if( obstacle < explorer->laser->max_range )
	    {
		/* Hack rápido para limpar o ponto do obstáculo */
		mp_map_mark_square( explorer,
				    x,
				    y,
				    OCCUPIED);
		/* Marca a parede */
		mp_map_mark_square( explorer,
				    x,
				    y,
				    OCCUPIED);
	    }
	}
	mp_map_image_update( explorer );
	    
	/* mp_walk_around( explorer ); */

	if( cvWaitKey(10) == 'q' )
	    break;
    }

    
    if( explorer != NULL )
	mp_destroy( explorer );
}

void
mp_walk_around( marco_polo_t * explorer )
{
    float left = 10;//menor distancia da esquerda
    float right = 10;//menor distancia da direita
    int i = 0;
     for ( i = 0; i <= 360; i++){//varre de meio em meio grau no explorer->laser
      if ( explorer->laser->scan[i][0] < 0.7 ){//verifica em cada posicao se a distancia eh menor que 0,7
        if (( i <= 180) && (explorer->laser->scan[i][0] < left)){
	  left = explorer->laser->scan[i][0];
	}else if ((i > 180)  && (explorer->laser->scan[i][0] < right)){
	  right = explorer->laser->scan[i][0];
	}
	if ( left < right){//se tah mais perto da esquerda
          playerc_position2d_set_cmd_vel(explorer->position2d, 0.2, 0, 0.5, 1);//vira pra direita
        }else if ( right <= left){//se tah mais perto da direita
          playerc_position2d_set_cmd_vel(explorer->position2d, 0.2, 0, -0.5, 1);//vira pra esquerda
        }
      }
    }
    if ((left >= 0.7) && (right >= 0.7)){
      playerc_position2d_set_cmd_vel(explorer->position2d, 0.2, 0, 0, 1);
    }
}

void
mp_map_image_update( marco_polo_t * explorer )
{
    int i = 0;
    int j = 0;

    for( i = 0 ; i < explorer->map_width ; i ++ )
    {
	for( j = 0 ; j < explorer->map_height ; j++ )
	{
	    if( explorer->map[i][j] > SQUARE_OCCUPIED )
	    {
		mp_map_image_mark_square( explorer,
					  i,
					  j,
					  OCCUPIED);
	    }
	    else if( explorer->map[i][j] < SQUARE_FREE )
	    {
		mp_map_image_mark_square( explorer,
					  i,
					  j,
					  FREE);
	    }else
	    {
		mp_map_image_mark_square( explorer,
					  i,
					  j,
					  UNKNOWN);
	    }
	}
    }
    cvShowImage( explorer->map_name,
		 explorer->map_image );
}

void
mp_map_x_y_obstacle( marco_polo_t * explorer,
		     double obs_dist,
		     double obs_angle,
		     int *x,
		     int *y)
{
    double px = 0;
    double py = 0;
    int map_pos_x = 0;
    int map_pos_y = 0;

    px = obs_dist*cos(obs_angle);
    py = obs_dist*sin(obs_angle);

    /* Descobrindo qual quadrado o explorador esta */
    map_pos_x = mp_map_get_pos_x( explorer );
    map_pos_y = mp_map_get_pos_y( explorer );

    /* Descobrindo qual quadrado o obstáculo esta */
    *x = (int) (map_pos_x + ( px/explorer->map_square_size));
    *y = (int) (map_pos_y + ( py/explorer->map_square_size));
    
}

void
mp_update_position( marco_polo_t * explorer )
{
    explorer->px = (int) rintf((explorer->position2d->px / explorer->map_square_size)
			       + explorer->map_pos_x_orig);
    explorer->py = (int) rintf((explorer->position2d->py / explorer->map_square_size)
			       + explorer->map_pos_y_orig);
}

int
mp_map_get_pos_x( marco_polo_t * explorer )
{
    return explorer->px;
}

int
mp_map_get_pos_y( marco_polo_t * explorer )
{
    return explorer->py;
}

/** Utilizando DDA  */
void
mp_map_mark_open_space( marco_polo_t * explorer,
			int px1,
			int py1 )
{
    int passo = 0;
    float x = 0;
    float y = 0;
    float m = 0;
    float inc_x = 0;
    float inc_y = 0;
    int i = 0;
    int px0 = 0;
    int py0 = 0;

    /* Descobrindo qual quadrado o explorador esta */
    px0 = mp_map_get_pos_x( explorer );
    py0 = mp_map_get_pos_y( explorer );

    /* Calculando m */
    m =	(float) (py1 - py0) / (px1 - px0);

    if (abs(m) < 1) {
    	passo = abs(px1 - px0);
    } else {
    	passo = abs(py1 - py0);
    }

    inc_x = (float) (px1 - px0) / passo;
    inc_y = (float) (py1 - py0) / passo;

    x =	px0;
    y =	py0;

    for (i = 0; i < passo; i++) {
    	x += inc_x;
    	y += inc_y;
    	mp_map_mark_square( explorer,
    			    rintf(x),
    			    rintf(y),
    			    FREE);
    }
}

void
mp_draw_map( marco_polo_t * explorer )
{
    int i = 0;

    for( i = 0; i < explorer->map_height ; i++ )
    {
	mp_map_image_mark_square( explorer,
				  i,
				  i,
				  UNKNOWN );
    }
    cvShowImage( explorer->map_name,
		 explorer->map_image );
}

void
mp_draw_pixel(marco_polo_t * explorer,
	      int x,
	      int y,
	      unsigned char r,
	      unsigned char g,
	      unsigned char b)
{
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)]=b;
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)+1]=g;
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)+2]=r;
}

void
mp_map_mark_square( marco_polo_t * explorer,
		    int x,
		    int y,
		    enum square_state state)
{

    y = explorer->map_height - y;
    switch (state)
    {
    case UNKNOWN:
    {
	explorer->map[x][y] = 0;
	break;
    }
    case FREE:
    {
	if( explorer->map[x][y] >  SQUARE_MIN_INC )
	    (explorer->map[x][y])--;
	break;
    }
    case OCCUPIED:
    {
	if( explorer->map[x][y] < SQUARE_MAX_INC )
	{
	    (explorer->map[x][y])++;
	    (explorer->map[x+1][y])++;
	    (explorer->map[x+1][y+1])++;
	    (explorer->map[x+1][y-1])++;
	    (explorer->map[x][y+1])++;
	    (explorer->map[x][y-1])++;
	    (explorer->map[x-1][y+1])++;
	    (explorer->map[x-1][y])++;
	    (explorer->map[x-1][y-1])++;
	}
	break;
    }
    }
}


void
mp_map_image_mark_square( marco_polo_t * explorer,
			  int x,
			  int y,
			  enum square_state state)
{
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)]=state;
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)+1]=state;
    explorer->map_image->imageData[3*((y*explorer->map_width)+x)+2]=state;
}

marco_polo_t *
mp_new( const char *server,
	const int port,
	const char * map_name)
{
    marco_polo_t * explorer = NULL;
    int i = 0;
    int j = 0;

    explorer = XMALLOC( marco_polo_t, 1 );

    explorer->client = NULL;
    explorer->laser = NULL;
    explorer->position2d = NULL;

    explorer->client = playerc_client_create( NULL,
					   server,
					   port );
    if( playerc_client_connect(explorer->client) )
    {
	xerror_error("Não consegui conectar no servidor");
	playerc_client_destroy( explorer->client );
	XFREE( explorer );
	return NULL;
    }
    
    explorer->laser = playerc_laser_create( explorer->client,
					 0 );
    if( playerc_laser_subscribe(explorer->laser, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o laser no servidor");
	playerc_laser_destroy( explorer->laser );
	playerc_client_disconnect( explorer->client );
	playerc_client_destroy( explorer->client );
	XFREE( explorer );
	return NULL;
    }
    
    explorer->position2d = playerc_position2d_create( explorer->client,
						   0 );
    if( playerc_position2d_subscribe(explorer->position2d, PLAYERC_OPEN_MODE) )
    {
	xerror_error("Não consegui registrar o odometro no servidor");
	playerc_position2d_destroy( explorer->position2d );
	playerc_laser_unsubscribe( explorer->laser );
	playerc_laser_destroy( explorer->laser );
	playerc_client_disconnect( explorer->client );
	playerc_client_destroy( explorer->client );
	XFREE( explorer );
	return NULL;
    }

    explorer->map_name = XMALLOC(char, strlen(map_name) + 1);
    strcpy( explorer->map_name, map_name );

    explorer->map_width = 800;
    explorer->map_height = 600;

    /* Setando as posicoes iniciais */
    explorer->map_pos_x_orig = (int) explorer->map_width / 2;
    explorer->map_pos_y_orig = (int) explorer->map_height / 2;

    /* Criando o mapa */
    explorer->map = XMALLOC( int*, explorer->map_width );
    for( i = 0 ; i < explorer->map_width ; i++ )
    {
	explorer->map[i] = XMALLOC( int, explorer->map_height );
    }

    /* Zerando o mapa */
    for( i = 0 ; i < explorer->map_width ; i++ )
	for( j = 0 ; j < explorer->map_height ; j++ )
	    mp_map_mark_square( explorer,
				i,
				j,
				UNKNOWN );
    
    /* Criando uma imagem do mapa */
    explorer->map_image = cvCreateImage(cvSize(explorer->map_width,
					 explorer->map_height),
				  IPL_DEPTH_8U,
				  3 );
    cvNamedWindow(explorer->map_name, CV_WINDOW_AUTOSIZE );

    /* Initiate map */
    for( i = 0 ; i < explorer->map_width ; i++ )
	for( j = 0 ; j < explorer->map_height ; j ++ )
	    mp_map_image_mark_square( explorer,
				      i,
				      j,
				      UNKNOWN );
    cvShowImage(explorer->map_name, explorer->map_image);

    /* Tamanho do quadrado */
    explorer->map_square_size = 0.1; /* 10 cm */

    /* Iniciando o HACK da posicao */
    explorer->px = 0;
    explorer->py = 0;
    

    return explorer;
}

void
mp_destroy( marco_polo_t * explorer )
{
    int i = 0;
    
    if( explorer->laser != NULL )
    {
	playerc_laser_unsubscribe( explorer->laser );
	playerc_laser_destroy( explorer->laser );
    }

    if( explorer->position2d != NULL )
    {
	playerc_position2d_unsubscribe( explorer->position2d );
	playerc_position2d_destroy( explorer->position2d );
    }

    if( explorer->client != NULL )
    {
	playerc_client_disconnect( explorer->client );
	playerc_client_destroy( explorer->client );
    }

    /* Liberando o mapa */
    for( i = 0 ; i < explorer->map_width ; i++ )
	XFREE( explorer->map[i] );
    XFREE( explorer->map );

	
    cvReleaseImage( &(explorer->map_image) );
    cvDestroyWindow( explorer->map_name );
    XFREE( explorer->map_name );
    XFREE( explorer );
}

/** Transforma ângulo do laser em ângulo global.
    
    O ângulo do laser é dado em radianos e tem como referência o
    robô. Esta função transforma-o em um ângulo com base no sistema
    global.

    @param pa Ângulo do robô no sistema global.
    @param laser Ângulo do laser no sistema do robô.

    @return Ângulo do laser no sistema global.
*/
double
mp_laser_to_global_rad( const double pa, const double laser )
{
    double angle = 0;

    angle = pa + laser;

    if( angle > M_PI )
    {
	angle = angle - M_PI;
	angle = -M_PI + angle;
    }else if (angle < -M_PI )
    {
	angle = angle + M_PI;
	angle = M_PI + angle;
    }
    return angle;
}


